/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package teamBlitz;

import com.sun.squawk.security.HexEncoding;
import edu.wpi.first.wpilibj.DigitalModule;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.SensorBase;
import edu.wpi.first.wpilibj.Timer;

/**
 *
 * @author dtjones
 */




public class MAG3110 extends I2CSync {

private static final byte BASE_ADDR = (byte)0x1c; //MAG address on i2c bus
private static final byte CNTRL2 = 0x11;              // cntrl register2
private static final byte CNTRL1 = 0x10;
private static final byte XLOW = 0x01;                //Y?
private static final byte YLOW = 0x01;

    //private I2C m_i2c;

    /**
     * Constructor.
     *
     * @param slot The slot of the digital module that the sensor is plugged into.
     * @param range The range (+ or -) that the accelerometer will measure.
     */
    float x,y,z;

    public float getX() {
        return x;
    }

    public float getY() {
        return y;
    }

    public float getZ() {
        return z;
    }
    
    public MAG3110(int slot) {
        super(DigitalModule.getInstance(slot),BASE_ADDR);
        boolean rc=false;
        byte[] input = new byte[2];        

        System.out.println("teamblitz mag3110 started");
//        DigitalModule module = DigitalModule.getInstance(slot);
//        System.out.println("module="+module);
        
//        m_i2c = module.getI2C(BASE_ADDR);
//        System.out.println("m_i2c="+m_i2c);
        rc=addressOnly();
         System.out.println("address bad="+rc);    
         
        //setCompatabilityMode(true);
       rc=read(0x07, 1,input); // check whoAmI
        System.out.println("WHO="+HexEncoding.hexEncode(input,1));  
        
        rc=write(CNTRL2, (byte)0x80); // send 0x80, enable auto resets
        System.out.println("CONTRL2="+rc);    
        Timer.delay(.1);   
        
        rc=write(CNTRL1, (byte)1); // send 0x80, enable auto resets
        System.out.println("CONTRL2="+rc);
         Timer.delay(.1);   
       
        rc=read(0x08, 1,input); // check op mode
        System.out.println("OP="+HexEncoding.hexEncode(input,1));  
         System.out.println("teamblitz mag3110 ended");
  }

   public boolean update() {
        byte[] input = new byte[2];        
        byte[] rawData = new byte[6];
        
        read(0x08, 1,input); // check op mode
        System.out.println("OP="+HexEncoding.hexEncode(input,1));  

        
        read(0x00, 1, input);
        System.out.println("data ready =" + input[0]);
       
        
        for(int i=0; i<6; i++)
        {
            read(XLOW+i, 1, input);
            rawData[i]=input[0];
            //System.out.println("return" + i + " ="+ rawData[i]);
        }
        x = rawData[1]+(rawData[0]<<8);
        y = rawData[3]+(rawData[2]<<8);
        z = rawData[5]+(rawData[4]<<8);
        return true;
   }
}
